#ifndef ARDUINO_LIB_HANDLE_CMD_CODEC_H
#define ARDUINO_LIB_HANDLE_CMD_CODEC_H

#include <Arduino.h>
#include "Motor.h"
#include "ServoChief.h"

//#define DEBUG_MODE

class HandleCmdCodec;

class HandleCmd
{
  public:
	friend HandleCmdCodec;
	HandleCmd() {}
	HandleCmd(const HandleCmd& c)
	{
	  cmdId = c.cmdId;
	  enFlag = c.enFlag;
	  motor = c.motor;
	  servo = c.servo;
	  id = c.id;
	  isValid = c.isValid;
	}
	HandleCmd(HandleCmd&& c)
	{
	  cmdId = c.cmdId;
	  enFlag = c.enFlag;
	  motor = c.motor;
	  servo = c.servo;
	  id = c.id;
	  isValid = c.isValid;
	}
	~HandleCmd() {}
	operator bool() const	 { return isValid; }
	HandleCmd& operator=(const HandleCmd& c)
	{
	  cmdId = c.cmdId;
	  enFlag = c.enFlag;
	  motor = c.motor;
	  servo = c.servo;
	  id = c.id;
	  isValid = c.isValid;
	  return *this;
	}
	void reset()
	{
	  cmdId = 0;
	  enFlag = 0;
	  motor = 0;
	  servo = 0;
	  id = 0;
	  isValid = false;
    mode = Motor::MotorMode::MODE_SHORT_BRAKE;
    turnMode_ = ServoChief::TurnMode::TURN_MODE_STANDBY;
	}
	int motorSpeed()
	{
		int speed;
		if(motor == kLeftJoystickMiddle || !enFlag) {
		  speed = 0;
		  mode = Motor::MotorMode::MODE_SHORT_BRAKE;
		}
		else if(motor > kLeftJoystickMiddle) {
		  speed = motor - kLeftJoystickMiddle;
		  speed = map(speed, 1, 127, 1, 255);
		  mode = Motor::MotorMode::MODE_CW;
		}
		else {
		  speed = kLeftJoystickMiddle - motor;
		  speed = map(speed, 1, 127, 1, 255);
		  mode = Motor::MotorMode::MODE_CCW;
		}
		if(speed > 255)
		  speed = 255;
		return speed;
	}
  ServoChief::TurnMode turnMode()
  {
    return turnMode_;
  }
  
  uint8_t turnAngle()
  {
    uint8_t angle;
    float ratio = 1.0*ServoChief::kMaxTurnAngle/(kRightJoystickMiddle + 1);

    if(servo == kRightJoystickMiddle || !enFlag) {
      angle = 0;
      turnMode_ = ServoChief::TurnMode::TURN_MODE_STANDBY;
    }
    else if(servo  > kRightJoystickMiddle) {
      angle = (servo - kRightJoystickMiddle) * ratio;
      turnMode_ = ServoChief::TurnMode::TURN_MODE_LEFT;
    }
    else {
      angle = (kRightJoystickMiddle - servo + 1) * ratio;
      turnMode_ = ServoChief::TurnMode::TURN_MODE_RIGHT;
    }
    if(angle > ServoChief::kMaxTurnAngle)
      angle = ServoChief::kMaxTurnAngle;
    return angle;
  }

	Motor::MotorMode motorMode()
	{
	  return mode;
	}
  protected:
	void decode(uint8_t buf[])
	{
	  //指令解码
	  cmdId = buf[0];
	  enFlag = bitRead(buf[1], 0);
	  motor = buf[2];
	  servo = buf[3];
	  id = buf[7];
	  //指令校验
	  if(cmdId != kCmdId || id != kId) {
      isValid = false;
      return;
	  }
	  isValid = true;
	}
  private:
	const uint8_t kId {27};
	const uint8_t kCmdId {0x88};
	const uint8_t kLeftJoystickMiddle {0x7c};
	const uint8_t kRightJoystickMiddle {0x7f};
	uint8_t cmdId {0};
	uint8_t enFlag : 1;
	uint8_t : 7;
	uint8_t motor {0};
	uint8_t servo {0};
	uint8_t id {0};
	bool isValid {false};
	Motor::MotorMode mode {Motor::MotorMode::MODE_SHORT_BRAKE};
  ServoChief::TurnMode turnMode_ {ServoChief::TurnMode::TURN_MODE_STANDBY};
};

class HandleCmdCodec
{
  public:
    HandleCmdCodec();
    ~HandleCmdCodec();
    HandleCmd getCmd();
  private:
    void printCmd();
  private:
    static const uint8_t kReceiveDelay {50-ServoChief::delayTime};
    static const int kBufferSize {8};
    //HandleCmd cmd;
    uint8_t buffer[kBufferSize];
};

#endif
